26. Predict Radar Measurement Assignment 1
Assignment Predict Radar Measurement
Predict Radar Measurements Assignment
Task Description:
You will be completing the missing code in ukf.cpp, PredictRadarMeasurement() function.
Task Feedback:
Nice! Have you tested your code below?
Helpful Equations
State Vector
x_{k+1|k}=\begin{bmatrix}
p_x\\
p_y\\
v\\
\psi\\
\dot{\psi}
\end{bmatrix}
Measurement Vector
z_{k+1|k}=\begin{bmatrix}
\rho\\
\varphi\\
\dot{\rho}
\end{bmatrix}
Measurement Model
z_{k+1|k}=h(x_{k+1}) + w_{k+1}
\rho = \sqrt{p_x^2+p_y^2}
\varphi =arctan(\frac{p_y}{p_x})
\dot{\rho}=\frac{p_xcos(\psi)v+p_ysin(\psi)v}{\sqrt{p_x^2+p_y^2}}
Predicted Measurement Mean
\large
z_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i Z_{k+1|k,i}
Predicted Covariance
\large
S_{k+1|k} = \sum_{i=1}^{n_\sigma} w_i( Z_{k+1|k,i} - z_{k+1|k})(Z_{k+1|k,i} - z_{k+1|k})^T + R
\large
R = E(w_k\cdot w_k^T) = \begin{bmatrix}
\sigma_{\rho}^2 \qquad 0\qquad0\\
0\qquad\sigma_{\varphi}^2 \qquad 0\\
0\qquad0\qquad\sigma_{\dot{\rho}}^2
\end{bmatrix}
Start Quiz:
#include "Dense"
#include "ukf.h"
using Eigen::MatrixXd;
using Eigen::VectorXd;
int main() {
// Create a UKF instance
UKF ukf;
/**
* Programming assignment calls
*/
VectorXd z_out = VectorXd(3);
MatrixXd S_out = MatrixXd(3, 3);
ukf.PredictRadarMeasurement(&z_out, &S_out);
return 0;
}
#include <iostream>
#include "ukf.h"
using Eigen::MatrixXd;
using Eigen::VectorXd;
UKF::UKF() {
Init();
}
UKF::~UKF() {
}
void UKF::Init() {
}
/**
* Programming assignment functions:
*/
void UKF::PredictRadarMeasurement(VectorXd* z_out, MatrixXd* S_out) {
// set state dimension
int n_x = 5;
// set augmented dimension
int n_aug = 7;
// set measurement dimension, radar can measure r, phi, and r_dot
int n_z = 3;
// define spreading parameter
double lambda = 3 - n_aug;
// set vector for weights
VectorXd weights = VectorXd(2*n_aug+1);
double weight_0 = lambda/(lambda+n_aug);
double weight = 0.5/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; ++i) {
weights(i) = weight;
}
// radar measurement noise standard deviation radius in m
double std_radr = 0.3;
// radar measurement noise standard deviation angle in rad
double std_radphi = 0.0175;
// radar measurement noise standard deviation radius change in m/s
double std_radrd = 0.1;
// create example matrix with predicted sigma points
MatrixXd Xsig_pred = MatrixXd(n_x, 2 * n_aug + 1);
Xsig_pred <<
5.9374, 6.0640, 5.925, 5.9436, 5.9266, 5.9374, 5.9389, 5.9374, 5.8106, 5.9457, 5.9310, 5.9465, 5.9374, 5.9359, 5.93744,
1.48, 1.4436, 1.660, 1.4934, 1.5036, 1.48, 1.4868, 1.48, 1.5271, 1.3104, 1.4787, 1.4674, 1.48, 1.4851, 1.486,
2.204, 2.2841, 2.2455, 2.2958, 2.204, 2.204, 2.2395, 2.204, 2.1256, 2.1642, 2.1139, 2.204, 2.204, 2.1702, 2.2049,
0.5367, 0.47338, 0.67809, 0.55455, 0.64364, 0.54337, 0.5367, 0.53851, 0.60017, 0.39546, 0.51900, 0.42991, 0.530188, 0.5367, 0.535048,
0.352, 0.29997, 0.46212, 0.37633, 0.4841, 0.41872, 0.352, 0.38744, 0.40562, 0.24347, 0.32926, 0.2214, 0.28687, 0.352, 0.318159;
// create matrix for sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug + 1);
// mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
// measurement covariance matrix S
MatrixXd S = MatrixXd(n_z,n_z);
/**
* Student part begin
*/
// transform sigma points into measurement space
// calculate mean predicted measurement
// calculate innovation covariance matrix S
/**
* Student part end
*/
// print result
std::cout << "z_pred: " << std::endl << z_pred << std::endl;
std::cout << "S: " << std::endl << S << std::endl;
// write result
*z_out = z_pred;
*S_out = S;
}
#ifndef UKF_H
#define UKF_H
#include "Dense"
class UKF {
public:
/**
* Constructor
*/
UKF();
/**
* Destructor
*/
virtual ~UKF();
/**
* Init Initializes Unscented Kalman filter
*/
void Init();
/**
* Student assignment functions
*/
void GenerateSigmaPoints(Eigen::MatrixXd* Xsig_out);
void AugmentedSigmaPoints(Eigen::MatrixXd* Xsig_out);
void SigmaPointPrediction(Eigen::MatrixXd* Xsig_out);
void PredictMeanAndCovariance(Eigen::VectorXd* x_pred,
Eigen::MatrixXd* P_pred);
void PredictRadarMeasurement(Eigen::VectorXd* z_out,
Eigen::MatrixXd* S_out);
void UpdateState(Eigen::VectorXd* x_out,
Eigen::MatrixXd* P_out);
};
#endif // UKF_H
expected result z_out:
z_pred =
6.12155
0.245993
2.10313
expected result s_out:
S =
0.0946171 -0.000139448 0.00407016
-0.000139448 0.000617548 -0.000770652
0.00407016 -0.000770652 0.0180917